#include "highLevel.h"

extern double processedDataBuffer[7];



void read(bool last,double lvData[],uint16_t depth)
{
    uint16_t samplesCollected = 0;
    READ_ENUM readStatus;
    while (samplesCollected < depth)
    {
 start:
        if(last)
        {
            //try to get the latest data
            readStatus = comPortGetLast();

            //if our data collection is up to date
            //revert to using standard comPortGet()
            if(readStatus == UP_TO_DATE)
            {
                readStatus = comPortGet();
            }

        }
        else
        {
            readStatus = comPortGet();
            asm("nop");
        }

        //further behavior will be based on the readStatus
        switch(readStatus)
        {
            //this will never happen because we check first

            case(UP_TO_DATE):
            asm("nop");
            //goto start;
            break;
            case(OTHER_MSG_FOUND):
            //no input data was present, but config data was
            asm("nop");
            break;
            case(MSG_NOT_FOUND):
            //no data was found.  If this is a common uccurance, than we may want to adjust our read last algorithim
            asm("nop");
            break;
            case(INPUT_MSG_FOUND):
            //input message found
            asm("nop");
            for(int i = 0; i < 7;i++)
            {
                lvData[samplesCollected++] = processedDataBuffer[i];
                if(i != 6)
                {
                    printf("% 1.3f\t",processedDataBuffer[i]);
                }
                else
                {
                    printf("0x%X\t",(uint16_t)processedDataBuffer[i]);
                }
            }
            printf("\r\n");
            //samplesCollected++;
            break;
            case(INPUT_AND_OTHER_MSG_FOUND):
            asm("nop");
            for(int i = 0; i < 7;i++)
            {
                lvData[samplesCollected++] = processedDataBuffer[i];
                if(i != 6)
                {
                    printf("% 1.3f\t",processedDataBuffer[i]);
                }
                else
                {
                    printf("0x%X\t",(uint16_t)processedDataBuffer[i]);
                }
            }
            printf("\r\n");
            //samplesCollected++;
            break;
        }

    }
}

void write(uint16_t servoVals[4],uint16_t motorVals[4], uint16_t analogOut[2],uint16_t dioSetMsk,uint16_t dioClrMsk)
{
    DAQ.sendActuatorSuperPacket(servoVals,motorVals);
    DAQ.sendOutputSuperPacket(analogOut,dioSetMsk,dioClrMsk);
}

void example()
{

    stopWatch delayTimer,timer2;
    double a0=0,a1=5;

    uint16_t analogOutVal[2];
    uint16_t servoVal[4],motorVal[4];
    uint16_t dioSetMsk,dioClrMsk;
    //delayTimer.start();
    timer2.start();
    uint32_t i = 0;
    bool direction = 1;
    while (timer2.split() < 25 )
    {

        while(bufRx.bytesInBuffer > packetSizeLookup(inputSuperPacket_mh))
        {
            comPortGet();
        }
        delayTimer.pause(.0001);

        a0 = 1*sin(2*3.14159*1*timer2.split())+1;
        a1 = 1.25*cos(2*3.14159*25*timer2.split())+1.25;
        analogOutVal[0] = round(a0/5.0*4095);
        analogOutVal[1] = round(a1/5.0*4095);

        if(direction)
        {
            dioSetMsk = ~(1<<i);
            dioClrMsk = (1<<i);

            if(i++ == 12)
            {
                i = 12;
                direction = 0;
            }
        }
        else
        {
            dioSetMsk = ~(1<<i);
            dioClrMsk = (1<<i);
            if(i-- < 3)
            {
                i = 2;
                direction = 1;
            }
        }

        write(servoVal,motorVal,analogOutVal,dioSetMsk,dioClrMsk);
        asm("nop");
    }
}


uint8_t init(   GAIN_ENUM ADC_GAIN[4]   ,
                uint8_t DAC_Enable_msk  ,
                PLOC_ENUM ENC_PLOC[2]   ,
                uint16_t ENC_RES[]      ,
                uint8_t servoMsk        ,
                uint8_t motorMsk        ,
                uint16_t inputPinMsk    ,
                uint16_t outputPinMsk   ,
                uint16_t freq           )
{

    uint8_t         ENC_INDX[2] = {0,0},
                    ENC_ABS[2]  = {0,0};

    DIGFILT_ENUM    ENC_DFLT[2] = {DIGFILT_8SAMPLES_gc,DIGFILT_8SAMPLES_gc};

    uint32_t i;

    stopWatch delayTimer,timer2;
    timer2.start();\

    do
    {
        DAQ.sendReset();// this is to reset the board software
        delayTimer.pause(.1);
        comPortGet();
    }while(DAQ.state != DAQ_INIT_STATE);

    printf("init state\r\n");

//    delayTimer.pause(.1);

    //analogInConfig
    for(i=0;i<4;i++)
    {
        DAQ.sendAdcInit(ADC_CH0_bp<<i,ADC_GAIN[i]);
    }

    //AnalogOutConfig
    DAQ.sendDacEnable(DAC_Enable_msk);


    //encoders
    for(i=0;i<2;i++)
    {
        DAQ.sendEncoderInit(ENC_PLOC[i],ENC_INDX[i],i,ENC_ABS[i],ENC_DFLT[i],ENC_RES[i]);
    }

    //actuators
    DAQ.sendServoEnable(servoMsk);
    DAQ.sendMotorEnable(motorMsk);

    //digital I/O
    DAQ.sendDigitalInEnable(inputPinMsk);
    DAQ.sendDigitalOutEnable(outputPinMsk);

    delayTimer.pause(.1);//was .05

    do
    {
        comPortGet();
    }while(bufRx.bytesInBuffer > 0);

    asm("nop");


    bool abort = 0;

    for(i=0;i<4;i++)
    {
        if(ADC_GAIN[i] != ADC_CH_GAIN_OFF_gc && DAQ.analogIn[i].enable != FEATURE_UTILIZED)
        {
            printf("ADC CH%d failed to init\r\n",i);
            abort = 1;
        }
    }
    printf("ADC initialized\r\n");

    if((DAC_Enable_msk & DAC_CH0_bp) && (DAQ.analogOut[0].enable != FEATURE_UTILIZED))
    {
        printf("DAC CH0 failed to init\r\n");
        abort = 1;
    }
    printf("DAC0 initialized\r\n");

    if(DAC_Enable_msk & DAC_CH1_bp && DAQ.analogOut[1].enable != FEATURE_UTILIZED)
    {
        printf("DAC CH1 failed to init\r\n");
        abort = 1;
    }
    printf("DAC1 initialized\r\n");

    for(i=0; i<13;i++)
    {


        if ( (inputPinMsk & 1<<i) && ((DAQ.digital.dioAlloc[i] != FEATURE_UTILIZED) || (DAQ.digital.src[i] != DIO_INPUT)))
        {
            printf("Input pin #%d failed to init\r\n",i);
            abort = 1;
        }
    }
    printf("DI initialized\r\n");

    for(i=0; i<13;i++)
    {
        if ( (outputPinMsk & 1<<i) && ((DAQ.digital.dioAlloc[i] != FEATURE_UTILIZED) || (DAQ.digital.src[i] != DIO_OUTPUT)))
        {
            printf("Output pin #%d failed to init\r\n",i);
            abort = 1;
        }
    }

    printf("DO initialized\r\n");

    for(i=0;i<2;i++)
    {
        if(ENC_PLOC[i] != PLOC_NULL && DAQ.encoder[i].enable != FEATURE_UTILIZED)
        {
            printf("Encoder #%d failed to init\r\n",i);
            abort = 1;
        }
    }
    printf("ENC initialized\r\n");

    for(i=0;i<4;i++)
    {
        if((servoMsk & 1<<i) && (DAQ.servo[i].enable != FEATURE_UTILIZED))
        {
            printf("Servo #%d failed to init\r\n",3-i);
            abort = 1;
        }
        if((motorMsk & 1<<i) && (DAQ.motor[i].enable != FEATURE_UTILIZED))
        {
            printf("Motor #%d failed to init\r\n",i);
            abort = 1;
        }
    }
    printf("ACT initialized\r\n");

    if(abort)
    {
        DAQ.sendReset();
        DAQ.sendStopProgram();
        FT_Close(handle);
        return 0;
    }


    DAQ.sendSetTimerFreq(freq);

    DAQ.sendStartProgram();
    return 1;
}
